#include <hand_driver/hand_driver_node.hpp>
#include "hand_driver/Index_Flexion.h"
#include "hand_driver/Index_PitchYaw.h"

// 遍历数组查找指定列的数据
int finder(float data, int item) {

    // 定义偏差值
    float tolerance1 = 10.0;  // 用于0的偏差
    float tolerance2 = 0.3;   // 用于1的偏差
    float tolerance;
    
    if (item < 0 || item >= INDEX_FLEXION_COLS) {
        return -1; // 列索引无效，返回 -1
    }
    
    if (item == 0) {
        tolerance = tolerance1;
    } else if (item == 1) {
        tolerance = tolerance2;
    } else {
        return -1;  // 无效的item值 -1
    }
    
    for (int i = 0; i < INDEX_FLEXION_ROWS; i++) {
        if (fabs(Index_Flexion[i][item] - data) <= tolerance) {
            return i;
        }
    }
    return -1; // 未找到返回 -1
}

int finder_ex(float data1, float data2, int item) {
    // 定义偏差值
    float tolerance1 = 10.0;  // 用于0和1列的偏差
    float tolerance2 = 0.3;   // 用于2和3列的偏差

    // 根据item值选择比较的列
    int col1, col2;
    float tolerance;
    
    if (item == 0) {
        col1 = 0;
        col2 = 1;
        tolerance = tolerance1;
    } else if (item == 1) {
        col1 = 2;
        col2 = 3;
        tolerance = tolerance2;
    } else {
        return -1;  // 无效的item值
    }

    // 遍历数组查找匹配项
    for (int i = 0; i < INDEX_PITCHYAW_ROWS; i++) {
        float diff1 = fabsf(Index_PitchYaw[i][col1] - data1);
        float diff2 = fabsf(Index_PitchYaw[i][col2] - data2);
        //printf("%d:%f %f\r\n",i,diff1,diff2);
        // 同时满足两个条件才返回匹配行号
        if (diff1 <= tolerance && diff2 <= tolerance) {
            return i;  // 返回匹配行的编号(从0开始)
        }
    }

    return -1;  // 未找到匹配项
}


class HandDriverNode : public rclcpp::Node
{
public:
    HandDriverNode() : Node("hand_driver_node")
    {
        int ret;

        // 读取参数
        declare_parameter("joint_names", std::vector<std::string>());
        declare_parameter("sensor_names", std::vector<std::string>());
        declare_parameter("motor_names", std::vector<std::string>());

        joint_names_ = get_parameter("joint_names").as_string_array();
        sensor_names_ = get_parameter("sensor_names").as_string_array();
        motor_names_ = get_parameter("motor_names").as_string_array();

        joint_count_ = joint_names_.size();
        RCLCPP_INFO(this->get_logger(), "joint_count: %d", joint_count_);

        motor_count_ = motor_names_.size();
        RCLCPP_INFO(this->get_logger(), "motor_count: %d", motor_count_);

        sensor_count_ = sensor_names_.size();
        RCLCPP_INFO(this->get_logger(), "sensor_count: %d", sensor_count_);

//=========================================CANFD及电缸初始化=========================================
        handle = cs_can_init(0, CAN_TYPE_CANFD_200U, 0, 500000, 2000000, 80, 80);
        //handle = cs_can_init(0, CAN_TYPE_CANFD_200U, 0, 500000, 4000000);
        if (!handle) 
        {
            RCLCPP_ERROR(this->get_logger(), "open can_device failed.");
        }

        // 接收电缸数据反馈帧(0x010)
        ret = cs_can_mailbox_reg(handle, 0, M_ADDR);
        RCLCPP_INFO(this->get_logger(), "cs_can_mailbox_reg for 0x010 returned: %d", ret);

        // 接收触觉传感器数据帧(0x011)
        ret = cs_can_mailbox_reg(handle, 1, S_ADDR);
        RCLCPP_INFO(this->get_logger(), "cs_can_mailbox_reg for 0x011 returned: %d", ret);

        // 电缸启动控制(0x400)
        canfd_txd[0] = 1;
        if(cs_can_send(handle, CANFD, CAN_FRAME_TYPE_STANDARD, SS_ADDR, canfd_txd, _CTRL_FRAME, 1000) < 0)
        {
            RCLCPP_ERROR(this->get_logger(), "set motor param failed!!!!");
        }
//---------------------------------------------------------------------------------------------------

        // 发布 joint_status 话题
        joint_status_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_status", 1);
        // 发布 sensor_status 话题
        sensor_status_pub_ = this->create_publisher<std_msgs::msg::Float32MultiArray>("sensor_status", 1);

        // 订阅自定义的关节控制话题
        // multi_array_sub_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
        //     "joint_controllers", 10, [this](const std_msgs::msg::Float32MultiArray::SharedPtr msg) {
        //         if (msg->data.size() == static_cast<size_t>(this->joint_count_)) {
        //             for (int i = 0; i < this->joint_count_; i++) {
        //                 switch (this->motor_[i].motor_mode) {
        //                     case MOTOR_POS_MODE:
        //                         this->joint_[i].prePOS = msg->data[i];
        //                         //RCLCPP_INFO(this->get_logger(), "joint_[%d] prePOS: %f", i, this->motor_[i].prePOS);
        //                         break;
        //                     case MOTOR_VEL_MODE:
        //                         this->joint_[i].preVEL = msg->data[i];
        //                         //RCLCPP_INFO(this->get_logger(), "joint_[%d] preVEL: %f", i, this->motor_[i].preVEL);
        //                         break;
        //                     case MOTOR_TOR_MODE:
        //                         this->joint_[i].preTOR = msg->data[i];
        //                         //RCLCPP_INFO(this->get_logger(), "joint_[%d] preTOR: %f", i, this->motor_[i].preTOR);
        //                         break;
        //                     default:
        //                         //RCLCPP_WARN(this->get_logger(), "joint_[%d] has an invalid motor_mode: %d", i, this->motor_[i].motor_mode);
        //                         break;
        //                 }
        //             }
        //         } 
        //         else
        //         {
        //             RCLCPP_WARN(this->get_logger(), "Received data length does not match motor_count");
        //         }
        //     });

            multi_array_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
                "joint_controllers", 10, [this](const sensor_msgs::msg::JointState::SharedPtr msg) {
                    // 检查消息中是否包含位置、速度或力矩数据
                    bool has_position = msg->position.size() >= static_cast<size_t>(this->joint_count_);
                    bool has_velocity = msg->velocity.size() >= static_cast<size_t>(this->joint_count_);
                    bool has_effort = msg->effort.size() >= static_cast<size_t>(this->joint_count_);
    
                    if (!has_position && !has_velocity && !has_effort) {
                        RCLCPP_WARN(this->get_logger(), "Received JointState message has no position, velocity, or effort data");
                        return;
                    }
    
                    // 根据电机控制模式分配数据
                    for (int i = 0; i < this->joint_count_; i++) {
                        switch (this->motor_[i].motor_mode) {
                            case MOTOR_POS_MODE:
                                if (has_position) {
                                    this->joint_[i].prePOS = msg->position[i];
                                } else {
                                    RCLCPP_WARN(this->get_logger(), "Position control mode but no position data for joint %d", i);
                                }
                                break;
                            case MOTOR_VEL_MODE:
                                if (has_velocity) {
                                    this->joint_[i].preVEL = msg->velocity[i];
                                } else {
                                    RCLCPP_WARN(this->get_logger(), "Velocity control mode but no velocity data for joint %d", i);
                                }
                                break;
                            case MOTOR_TOR_MODE:
                                if (has_effort) {
                                    this->joint_[i].preTOR = msg->effort[i];
                                } else {
                                    RCLCPP_WARN(this->get_logger(), "Torque control mode but no effort data for joint %d", i);
                                }
                                break;
                            default:
                                RCLCPP_WARN(this->get_logger(), "Joint %d has invalid motor_mode: %d", i, this->motor_[i].motor_mode);
                                break;
                        }
                    }
                });

        // 创建定时器，定期发布话题
        timer_js_ = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&HandDriverNode::publishJointStatus, this));
        timer_ss_ = this->create_wall_timer(std::chrono::milliseconds(50), std::bind(&HandDriverNode::publishSensorStatus, this));
        timer_sync_ = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&HandDriverNode::sync_data, this));
    }

    ~HandDriverNode()
    {
//=========================================CANFD及电缸反初始化=========================================
        if (handle) 
        {
            // 电缸停止控制(0xF00)
            canfd_txd[0] = 0;
            if(cs_can_send(handle, CANFD, CAN_FRAME_TYPE_STANDARD, SS_ADDR, canfd_txd, _CTRL_FRAME, 1000) < 0)
            {
                RCLCPP_ERROR(this->get_logger(), "set close motor param failed!!!!");
            }

            // 注销注册的CAN接收邮箱
            cs_can_mailbox_unreg(handle, 0);
            cs_can_mailbox_unreg(handle, 1);
            
            // 关闭CANFD设备
            printf("close can_device.\n");
            cs_can_uninit(handle);
        }
        RCLCPP_INFO(this->get_logger(), "[HandDriverNode] exit.");
//-----------------------------------------------------------------------------------------------------
    }

private:

    int joint2motor(void)
    {
        // 关节控制量转换成电机的控制量
        // demo1:
        for (int idx = 0; idx < motor_count_; idx++)
        {
            switch(motor_[idx].motor_mode)
            {
                case MOTOR_POS_MODE:
                    motor_[idx].target_position = joint_[idx].prePOS;
                    break;
                case MOTOR_VEL_MODE:
                    motor_[idx].target_velocity = joint_[idx].preVEL;
                    break;
                case MOTOR_TOR_MODE:
                    motor_[idx].target_force = joint_[idx].preTOR;
                    break;
            }
        }
        
        //demo2:
        int index;
        index = finder(joint_[3].prePOS, 1);
        if (index>=0)
        {
          //RCLCPP_INFO(this->get_logger(), "row: %d data:%f", index, joint_[3].prePOS);
          motor_[2].target_position = Index_Flexion[index][0];
        }
        
        return 0;
    }

    int motor2joint(void)
    {
        // 电机控制量转换成关节的控制量
        //demo 
        for (int idx = 0; idx < motor_count_; idx++)
        {
            switch(motor_[idx].motor_mode)
            {
                case MOTOR_POS_MODE:
                    joint_[idx].actPOS = motor_[idx].actual_position;
                    break;
                case MOTOR_VEL_MODE:
                    joint_[idx].actVEL = motor_[idx].actual_velocity;
                    break;
                case MOTOR_TOR_MODE:
                    joint_[idx].actTOR = motor_[idx].actual_force;
                    break;
            }
            
        }
        
        //demo2:
        int index;
        index = finder(joint_[3].actPOS, 0);
        if (index>=0)
        {
          //RCLCPP_INFO(this->get_logger(), "row: %d data:%f", index, joint_[3].actPOS);
          motor_[2].actual_position = Index_Flexion[index][1];
        }
        
        return 0;
    }

    void publishJointStatus()
    {
        auto joint_status_msg = std::make_unique<sensor_msgs::msg::JointState>();
        joint_status_msg->header.stamp = this->get_clock()->now();
        joint_status_msg->name = joint_names_;

        // 清空向量
        joint_status_msg->position.clear();
        joint_status_msg->velocity.clear();
        joint_status_msg->effort.clear();

        // 

        // 根据 joint_names_ 的数量，依次将 joint_ 的 actPOS、actVEL、actTOR 赋值
        for (size_t i = 0; i < joint_count_; i++) 
        {
            joint_status_msg->position.push_back(joint_[i].actPOS);
            joint_status_msg->velocity.push_back(joint_[i].actVEL);
            joint_status_msg->effort.push_back(joint_[i].actTOR);
        }

        joint_status_pub_->publish(std::move(joint_status_msg));
    }
    
    void publishSensorStatus()
    {  
        auto sensor_status_msg = std::make_unique<std_msgs::msg::Float32MultiArray>();
        sensor_status_msg->data.resize(sensor_count_*_SENSOR_DIM);
        for (int i = 0; i < sensor_count_*_SENSOR_DIM; i++) 
        {
            sensor_status_msg->data[i] = sensor_[i].data;
        }
        sensor_status_pub_->publish(std::move(sensor_status_msg));
    }

    void sync_data()
    {

//=============================================电缸数据读写====================================================
        //向CAN卡写入控制数据
        joint2motor();
        for (size_t i = 0; i < motor_count_; i++) 
        {
            short value;
            switch(motor_[i].motor_mode)
            {
                case MOTOR_POS_MODE:
                    value = static_cast<short>(motor_[i].target_position);
                    break;
                case MOTOR_VEL_MODE:
                    value = static_cast<short>(motor_[i].target_velocity);
                    break;
                case MOTOR_TOR_MODE:
                    value = static_cast<short>(motor_[i].target_force);
                    break;
            }
            size_t index = i * 2;
            canfd_txd[index] = static_cast<unsigned char>((value >> 8) & 0xFF);
            canfd_txd[index + 1] = static_cast<unsigned char>(value & 0xFF);
        }       
        if (cs_can_send(handle, CANFD, CAN_FRAME_TYPE_STANDARD, DT_ADDR, canfd_txd, _MOTOR_FRAME, 1000) < 0)
        {
            RCLCPP_ERROR(this->get_logger(), "Send 0x300 failed!!!.");
        }

        // 从CAN卡读取电缸反馈数据
        if (cs_can_recv(handle, _MOTOR_MAILBOX, CANFD, CAN_FRAME_TYPE_STANDARD, M_ADDR, canfd_rxd, _MOTOR_FRAME, 0) > 0)
        {
           // RCLCPP_INFO(this->get_logger(), "recv can(0x%03x): %02x %02x %02x %02x %02x %02x", M_ADDR, canfd_rxd[0], canfd_rxd[1], canfd_rxd[2], canfd_rxd[3], canfd_rxd[4], canfd_rxd[5]);

            for (size_t i = 0; i < motor_count_; i++)
            {
                if (i * 2 + 1 < _MOTOR_FRAME)
                {
                    short tmp = canfd_rxd[i * 2];
                    tmp = tmp << 8;
                    tmp = tmp | canfd_rxd[i * 2 + 1];
 
                    switch(motor_[i].motor_mode)
                    {
                        case MOTOR_POS_MODE:
                            motor_[i].actual_position = tmp;
                            break;
                        case MOTOR_VEL_MODE:
                            motor_[i].actual_velocity = tmp;
                            break;
                        case MOTOR_TOR_MODE:
                            motor_[i].actual_force = tmp;
                            break;
                        default:
                            RCLCPP_ERROR(this->get_logger(), "motor_mode failed!!!."); 
                            break;
                    }
                }
                else
                {
                    RCLCPP_ERROR(this->get_logger(), "Insufficient data in canfd_rxd for motor %zu", i);
                    break;
                }
            }
            motor2joint();
        }
 
        //  从CAN卡读取传感器反馈数据
        if (cs_can_recv(handle, _SENSOR_MAILBOX, CANFD, CAN_FRAME_TYPE_STANDARD, S_ADDR, canfd_rxd, _SENSOR_FRAME, 0) > 0)
        {
            for (int i = 0; i < sensor_count_*_SENSOR_DIM; i++) 
            {
                // 传感器的2，5，8，11，15数据类型是unsigned cha
                if (i == 2 || i == 5 || i == 8 || i == 11 || i == 14) 
                {
                    sensor_[i].data = static_cast<unsigned char>(canfd_rxd[i]);
                } 
                else 
                {
                    sensor_[i].data = static_cast<char>(canfd_rxd[i]);
                }
            }
        }
//-------------------------------------------------------------------------------------------------------------

    }

    void *handle;

    unsigned char joint_count_;
    unsigned char motor_count_;
    unsigned char sensor_count_;

    motor_t motor_[_MOTOR_MAX];
    joint_t joint_[_JOINT_MAX];
    sensor_t sensor_[_SENSOR_MAX];

    unsigned char canfd_txd[64];
    unsigned char canfd_rxd[64];
    std::vector<std::string> joint_names_;
    std::vector<std::string> motor_names_;
    std::vector<std::string> sensor_names_;

    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_status_pub_;
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr sensor_status_pub_;
    // rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr multi_array_sub_;
    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr multi_array_sub_;

    rclcpp::TimerBase::SharedPtr timer_js_;
    rclcpp::TimerBase::SharedPtr timer_ss_;
    rclcpp::TimerBase::SharedPtr timer_sync_;

};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<HandDriverNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}    
